#include <cmath>
#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#define PI 3.1415926535898
#define DEC (PI / 180)
double fx = 5.63;
double fy = 5.63;
double cx = 2;
double cy = 1;
class DSwitch {
   private:
    cv::Point2i startPoint;
    double R;
    double angleY;

   public:
    //构造方法,-空参构造与全参构造
    DSwitch() {}
    DSwitch(double x, double y, double r, double angle) {
        startPoint.x = x;
        startPoint.y = y;
        R = r;
        angleY = angle;
    }
    //提供对应的set与get方法
    void setStartPoint(double x, double y) {
        startPoint.x = x;
        startPoint.y = y;
    }
    cv::Point2i getStartPoint() { return startPoint; }
    void setR(double r) { R = r; }
    double getR() { return R; }
    void setAngleY(double angle) { angleY = angle; }
    double getAngleY() { return angleY; }

    //实现点在不同坐标系下变化的函数
    cv::Point3f change() {
        //给定一个点，假想下实现坐标的转化
        //初始化相关参数 // X = (F*W) / P
        double Z = (fx * 0.15) / 2 * R;
        //像素坐标到相机坐标
        // 1.初始化内参矩阵
        cv::Mat A(cv::Matx33d(fx, 0, cx, 0, fy, cy, 0, 0, 1));
        // 2.内参数矩阵乘以对应向量，处理后得到初始相机坐标
        cv::Mat begin(3, 1, cv::DataType<double>::type);
        begin.at<double>(0, 0) = startPoint.x;
        begin.at<double>(1, 0) = startPoint.y;
        begin.at<double>(2, 0) = 1;
        //右手执教坐标系变成了左手执教坐标系，比例关系发生了改变，把最后的变化用一个矩阵表示出来
        cv::Mat special(3, 3, cv::DataType<double>::type);
        special.at<double>(0, 0) = 0;
        special.at<double>(0, 1) = 1;
        special.at<double>(0, 2) = 0;
        special.at<double>(1, 0) = 1;
        special.at<double>(1, 1) = 0;
        special.at<double>(1, 2) = 0;
        special.at<double>(2, 0) = 0;
        special.at<double>(2, 1) = 0;
        special.at<double>(2, 2) = 1;
        cv::Mat B;
        A = A * special;
        cv::invert(A, B);
        cv::Mat result1 = Z * B * begin;
        // 3.相机旋转一定角度，计算出旋转之后的坐标
        // 3.1计算矩阵R1，R2，R3并相乘
        cv::Mat rvecM2(3, 3, cv::DataType<double>::type);  // yaw的旋转矩阵
        cv::Mat rvecM4(3, 3, cv::DataType<double>::type);  //特殊的旋闸u矩阵可以实现从右到左或从左到右

        rvecM2.at<double>(0, 0) = cos(angleY * DEC);
        rvecM2.at<double>(0, 1) = -sin(angleY * DEC);
        rvecM2.at<double>(0, 2) = 0;
        rvecM2.at<double>(1, 0) = sin(angleY * DEC);
        rvecM2.at<double>(1, 1) = cos(angleY * DEC);
        rvecM2.at<double>(1, 2) = 0;
        rvecM2.at<double>(2, 0) = 0;
        rvecM2.at<double>(2, 1) = 0;
        rvecM2.at<double>(2, 2) = 1;

        rvecM4.at<double>(0, 0) = 1;
        rvecM4.at<double>(0, 1) = 0;
        rvecM4.at<double>(0, 2) = 0;
        rvecM4.at<double>(1, 0) = 0;
        rvecM4.at<double>(1, 1) = 1;
        rvecM4.at<double>(1, 2) = 0;
        rvecM4.at<double>(2, 0) = 0;
        rvecM4.at<double>(2, 1) = 0;
        rvecM4.at<double>(2, 2) = -1;
        // cv::Mat R = rvecM4 * rvecM3 * rvecM2 * rvecM1 * rvecM4;
        cv::Mat R =
            rvecM4 * rvecM2 * rvecM4;  //坐标先转到右手，然后乘以右手的旋转矩阵，在乘以从右手到左手的一个变换
        // 3.2实现坐标的转化，并输出
        cv::Mat result2 = R * result1;
        cv::Point3f lastPoint;
        lastPoint.x = result2.at<double>(0, 0);
        lastPoint.y = result2.at<double>(1, 0);
        lastPoint.z = result2.at<double>(2, 0);
        return lastPoint;
    }
};

int main() {
    // //给定一个点，假想下实现坐标的转化
    // cv::Point2i startPoint;
    // double r;
    // std::cout << "输入像素点坐标(x,y)" << std::endl;
    // std::cin >> startPoint.x >> startPoint.y;
    // std::cout << "输入球的像素半径：" << std::endl;
    // std::cin >> r;
    // // std::cout << "(" << p.x << "," << p.y << ")" << std::endl;
    // //初始化相关参数
    // double fx = 5.63;
    // double fy = 5.63;
    // double cx = 2;
    // double cy = 1;
    // double Z = (fx * 0.075) / r;
    // std::cout << Z << std::endl;
    // // X = (F*W) / P
    // //像素坐标到相机坐标
    // // 1.初始化内参矩阵
    // cv::Mat A(cv::Matx33d(fx, 0, cx, 0, fy, cy, 0, 0, 1));
    // // 2.内参数矩阵乘以对应向量，处理后得到初始相机坐标
    // cv::Mat begin(3, 1, cv::DataType<double>::type);
    // begin.at<double>(0, 0) = startPoint.x;
    // begin.at<double>(1, 0) = startPoint.y;
    // begin.at<double>(2, 0) = 1;
    // //右手执教坐标系变成了左手执教坐标系，比例关系发生了改变，把最后的变化用一个矩阵表示出来
    // cv::Mat special(3, 3, cv::DataType<double>::type);
    // special.at<double>(0, 0) = 0;
    // special.at<double>(0, 1) = 1;
    // special.at<double>(0, 2) = 0;
    // special.at<double>(1, 0) = 1;
    // special.at<double>(1, 1) = 0;
    // special.at<double>(1, 2) = 0;
    // special.at<double>(2, 0) = 0;
    // special.at<double>(2, 1) = 0;
    // special.at<double>(2, 2) = 1;
    // cv::Mat B;
    // A = A * special;
    // cv::invert(A, B);
    // cv::Mat result1 = Z * B * begin;
    // // 3.相机旋转一定角度，计算出旋转之后的坐标
    // double angleY;
    // std::cout << "angleY:" << std::endl;
    // std::cin >> angleY;
    // // 3.1计算矩阵R1，R2，R3并相乘
    // cv::Mat rvecM2(3, 3, cv::DataType<double>::type);  // yaw的旋转矩阵
    // cv::Mat rvecM4(3, 3, cv::DataType<double>::type);  //特殊的旋闸u矩阵可以实现从右到左或从左到右

    // rvecM2.at<double>(0, 0) = cos(angleY * DEC);
    // rvecM2.at<double>(0, 1) = 0;
    // rvecM2.at<double>(0, 2) = sin(angleY * DEC);
    // rvecM2.at<double>(1, 0) = 0;
    // rvecM2.at<double>(1, 1) = 1;
    // rvecM2.at<double>(1, 2) = 0;
    // rvecM2.at<double>(2, 0) = -sin(angleY * DEC);
    // rvecM2.at<double>(2, 1) = 0;
    // rvecM2.at<double>(2, 2) = cos(angleY * DEC);

    // rvecM4.at<double>(0, 0) = 1;
    // rvecM4.at<double>(0, 1) = 0;
    // rvecM4.at<double>(0, 2) = 0;
    // rvecM4.at<double>(1, 0) = 0;
    // rvecM4.at<double>(1, 1) = 1;
    // rvecM4.at<double>(1, 2) = 0;
    // rvecM4.at<double>(2, 0) = 0;
    // rvecM4.at<double>(2, 1) = 0;
    // rvecM4.at<double>(2, 2) = -1;
    // // cv::Mat R = rvecM4 * rvecM3 * rvecM2 * rvecM1 * rvecM4;
    // cv::Mat R =
    //     rvecM4 * rvecM2 * rvecM4;  //坐标先转到右手，然后乘以右手的旋转矩阵，在乘以从右手到左手的一个变换
    // // 3.2实现坐标的转化，并输出
    // cv::Mat result2 = R * result1;
    // cv::Point3f lastPoint;
    // lastPoint.x = result2.at<double>(0, 0);
    // lastPoint.y = result2.at<double>(1, 0);
    // lastPoint.z = result2.at<double>(2, 0);
    // std::cout << lastPoint.x << std::endl;
    // std::cout << lastPoint.y << std::endl;
    // std::cout << lastPoint.z << std::endl;
    DSwitch p(10, 20, 10, 90);
    cv::Point3f p1 = p.change();
    std::cout << p1.x << std::endl << p1.y << std::endl << p1.z << std::endl;
}
